#include <cmath>
#include <iostream>

#include "Node.h"
#include "TopologicalObject.h"
#include "FEMModel.h"
#include "Tetrahedron.h"
#include "Triangle.h"
#include "Component.h"
#include "Setting.h"

void RemoveComponents (LinkArray<Tetrahedron>const*);  
void FindSmallVolumes (LinkArray<Tetrahedron>*,ComponentMap const*); 

using namespace std;


/*
Union-find on the set of tetrahedra.
*/
TopologicalObject* FindClassRepresentative (ClassMap *cmap, TopologicalObject * n)
{
	ClassMap::const_iterator i = cmap->find (n);
	if (i == cmap->end ())
	{
		(*cmap)[n] = n;
		return n;
	}

	/*
	find representative
	*/
	TopologicalObject * rep = n; 
	while ((*cmap)[rep] != rep)
	{
		rep = (*cmap)[rep];
	}

	/*
	path compression
	*/       
	while ((*cmap)[n] != rep)
	{
		TopologicalObject *next = (*cmap)[n];
		(*cmap)[n] = rep;
		n = next;
	}

	return rep;
}


bool FixedTetB (TopologicalObject*top)
{
	Tetrahedron* tet = dynamic_cast<Tetrahedron*> (top);

	int f = 0;
	for(int i =4;i--;)
		if (tet->GetNode (i)->FixedB ())
			f++;

	return f >= 3;
}

void ConnectClasses (ClassMap *cmap, TopologicalObject *n1, TopologicalObject* n2)
{
	TopologicalObject * t1 = FindClassRepresentative (cmap, n1);
	TopologicalObject * t2 = FindClassRepresentative (cmap, n2);

	if (t1 != t2)
	{
		if (FixedTetB (t1))
		{
			(*cmap)[t2] = t1;
		}
		else
		{
			(*cmap)[t1] = t2;
		}
	}
}

ClassMap * FindClasses (TopologicalModel *model)
{
	/*
	Finding components only works if cached links  are OK.
	*/
	model->ValidateCachedLinks (); // ugh, needed?

	LinkArray<Tetrahedron> ts ( model->TetrahedronArray ());

	ClassMap * cm  = new ClassMap;  
	for (int i = ts.Size (); i-- ;)
	{
		Tetrahedron * tet = ts[i];

		/*
		force TET to be in  CM
		*/
		FindClassRepresentative (cm, tet);

		for (int j=0; j < 4; j++)
		{
			Triangle * t = tet->GetTriangle (j);

			if (t->Mate ())
				ConnectClasses (cm, t->Mate()->Tetra (), tet);
		}
	}
	return cm;
}

ComponentMap *FindComponents (ClassMap * classes)
{
	ComponentMap * components  = new ComponentMap;
	for (ClassMap::const_iterator ki (classes->begin ()); ki!=classes->end ();
		ki++)
	{
		TopologicalObject *repr = FindClassRepresentative (classes, ki->second);

		ComponentMap ::const_iterator km =
			components->find (repr);

		ObjectSet *ap=0;
		if (km != components->end () && (*km).second)
		{
			ap = (*km).second;
		}
		else
		{
			ap = new ObjectSet;
			(*components)[repr] = ap;
		}

		ap->insert (ki->first);
	}
	return components;
}


/*
Find face-connected tetrahedra 
*/
ComponentMap *FindComponents (TopologicalModel *model)
{
	ClassMap * cm = FindClasses (model);
	ComponentMap * cs =  FindComponents (cm);
	delete cm;

	return cs;
}


/*
Given the component map CS, we check each component for fixedness.
*/
void FixLooseComponents (FiniteElementModel*fem, ComponentMap const *cs)
{
	int count=0 ;
	for (ComponentMap::const_iterator i ( cs->begin());
		i != cs->end (); i++)
	{
		count ++;
		TopologicalObject * t = i->first ;
		if (!FixedB (t))
		{
			cout << "Unfixed tetrahedron found.  Fixing. "<<endl;
			cout << "tetrahedron no. "<< t->Number ()  <<endl;

			for (int k = 0; k < 3; k++)
			{
				for (int j =0 ; j<3; j++)
					fem->FixNode (t->GetNode (k), true);
			}
		}
	}

	cout << "Found " << count << " connected component"  << ((count > 1) ? "s" : "") << endl;
}


void PrintComponents (ComponentMap const*cs)
{
	LinkArray<TopologicalObject> component_reprs ;

	/*
	Store the key fields of CS in separate list (COMPONENTS), since we
	have to delete stuff from CS.  */
	for (ComponentMap::const_iterator i ( cs->begin());
		i != cs->end (); i++)
		component_reprs.Push ((*i).first);

	for (int i = component_reprs.Size (); i--;)
	{
		Real vol = 0.0;
		int sz = 0;

		TopologicalObject *t =component_reprs[i];
		ComponentMap::const_iterator it =  cs->find (t);
		ObjectSet * oset = (*it).second;

		for (ObjectSet::const_iterator j (oset->begin ());
			j != oset->end (); j++)
		{
			Tetrahedron * t = dynamic_cast<Tetrahedron*> ((*j) );
			vol += fabs (t->Volume (false));
			sz++;
		}

		cout << "Component, represented by tet " <<  t->Number () << " set = " << t->GetSet () << " count = " << sz << ", volume = " << vol  <<endl;
	}  
}



/*
Check that all components have a fixed node, junk components that
are too small to be interesting. Any removed component is also
deleted from CS.
*/
void RemoveSmallComponents (FiniteElementModel *fem, ComponentMap * cs,
						 Real minvol)
{
	if (minvol <=0)
		return ;
	/*
	put in DELTETS all tetrahedra belong to a component that is smaller
	than MINIMAL_COMPONENT_VOLUME.
	*/
	LinkArray<Tetrahedron> deltets;
	LinkArray<TopologicalObject> component_reprs ;

	/*
	Store the key fields of CS in separate list (COMPONENTS), since we
	have to delete stuff from CS.  */
	for (ComponentMap::const_iterator i ( cs->begin());
		i != cs->end (); i++)
		component_reprs.Push ((*i).first);

	for (int i = component_reprs.Size (); i--;)
	{
		Real vol = 0.0;
		int sz = 0;

		TopologicalObject *t =component_reprs[i];
		ObjectSet * oset = (*cs) [t];

		for (ObjectSet::const_iterator j (oset->begin ());
			j != oset->end (); j++)
		{
			Tetrahedron * t = dynamic_cast<Tetrahedron*> ((*j) );
			vol += fabs (t->Volume (false));
			sz++;
		}

		cout << "Component, represented by tet " <<  t->Number () << " set = " << t->GetSet () << " count = " << sz << ", volume = " << vol  <<endl;

		if (vol < minvol)
		{
			int size =0;

			for (ObjectSet::const_iterator j (oset->begin ());
				j != oset->end (); j++)
			{
				Tetrahedron * t = dynamic_cast<Tetrahedron*> (*j);
				deltets.Push (t);
				size ++;
			}

			cout << "Removing component of " << size << " tets "<<endl;

			cs->erase (component_reprs[i]);
		}
	}

	if (deltets.Size( ))
		fem->RemoveComponents (&deltets);
}